//
// Created by 26090 on 24-12-17.
//

#include "mastermachine.h"

#include <bsp_dwt.h>
#include <string.h>
#include <sys/types.h>

#include "bsp_usart.h"

#define STOP_TIME 3.5f

#define VISION_RECV_SIZE 40;

#define DIRECTION DIRECTION_CW

#pragma pack(1)

static uint32_t color_counter,zero_counter;
static Color_e current_color;
static float stop_time_start,stop_time_end;
static float stop_length,start_length;
static uint8_t use_self_counter=0;

struct master_recv_1
{
    uint8_t header[2];
    int8_t cmd;
    int8_t is_stop;
    int8_t direction;
    int8_t color[3];
    int16_t xpos,ypos;
}master_recv_1;

struct master_recv_2
{
    uint8_t header[2];
    int8_t cmd;
    int16_t xpos,ypos;
}master_recv_2;


#pragma pack()

void DecodeMaster(struct _usart_instance* arg);

static USARTInstance *master_usart_instance;

static Master_recv_s recv_data;
static Master_send_s send_data;

static bool master_is_registered=false;

Master_recv_s *CommunicateInit(UART_HandleTypeDef *_handle)
{
    if(master_is_registered)
    {
        return &recv_data;
    }
    USART_Init_Config_s conf={0};
    conf.module_callback = DecodeMaster;
    conf.recv_buff_size = VISION_RECV_SIZE;
    conf.usart_handle = _handle;
    conf.duplex_mode = USART_DUPLEX_FULL;
    conf.tx_fifo_mode = USART_FIFO_DISABLE;
    master_usart_instance = USARTRegister(&conf);

    // 为master process注册daemon,用于判断视觉通信是否离线
    // Daemon_Init_Config_s daemon_conf = {
    //     .callback = VisionOfflineCallback, // 离线时调用的回调函数,会重启串口接收
    //     .owner_id = vision_usart_instance,
    //     .reload_count = 10,
    // };
    // vision_daemon_instance = DaemonRegister(&daemon_conf);

    master_is_registered = true;
    return &recv_data;
}
static uint32_t master_recv_count;
void DecodeMaster(struct _usart_instance* arg)
{

    const uint8_t *receive_buff=arg->recv_buff;
    if(receive_buff[0]==0x6F && receive_buff[1]==0x5A)          //上位机发送消息
    {
        master_recv_count++;
        uint8_t cmd=receive_buff[2];
        switch(cmd)
        {

        case 1:

            break;
        case 2:
            memcpy(&master_recv_1, receive_buff, sizeof(master_recv_1));

            static uint8_t checking_flag =0;
            if(!use_self_counter)                           //注释此条
            // if(1)                                           //取消注释词条
            {


                recv_data.last_is_stop = recv_data.is_stop;

                if(recv_data.is_stop==0 && master_recv_1.is_stop==1)
                {
                    checking_flag++;
                }
                //因为停转较为可靠
                if(!master_recv_1.is_stop)
                {
                    recv_data.is_stop=0;
                    checking_flag=0;

                }
                if(checking_flag)
                {

                    if(checking_flag>=10)
                    {
                        recv_data.is_stop=1;
                        //说明新一次的停止开始了
                        stop_time_start = DWT_GetTimeline_s();
                        // stop_time_end = stop_time_start + STOP_TIME;             //取消注释
                        checking_flag=0;
                    }
                }

                // recv_data.direction = (uint8_t)master_recv_1.direction;
                recv_data.direction = DIRECTION;
                // recv_data.direction = 0;

                if(recv_data.is_stop==1 && recv_data.last_is_stop==0)
                {

                    recv_data.color[LAST][0]=recv_data.color[CURRENT][0];
                    recv_data.color[LAST][1]=recv_data.color[CURRENT][1];
                    recv_data.color[LAST][2]=recv_data.color[CURRENT][2];

                    recv_data.color[CURRENT][0]=master_recv_1.color[0];
                    recv_data.color[CURRENT][1]=master_recv_1.color[1];
                    recv_data.color[CURRENT][2]=master_recv_1.color[2];

                    if(recv_data.direction == DIRECTION_CW)
                    {
                        recv_data.color[FUTURE][0]=recv_data.color[CURRENT][1];
                        recv_data.color[FUTURE][1]=recv_data.color[CURRENT][2];
                        recv_data.color[FUTURE][2]=recv_data.color[CURRENT][0];
                    }
                    else if(recv_data.direction == DIRECTION_CCW)
                    {
                        recv_data.color[FUTURE][0]=recv_data.color[CURRENT][2];
                        recv_data.color[FUTURE][1]=recv_data.color[CURRENT][0];
                        recv_data.color[FUTURE][2]=recv_data.color[CURRENT][1];
                    }

                }
                if(recv_data.is_stop==0 && recv_data.last_is_stop==1)
                {
                    stop_time_end=DWT_GetTimeline_s();                      //注释
                }
            }



            recv_data.ydis = master_recv_1.xpos;
            recv_data.xdis = master_recv_1.ypos;
            break;
        case 3:
            memcpy(&master_recv_2, receive_buff, sizeof(master_recv_2));
            recv_data.ydis = master_recv_2.xpos;
            recv_data.xdis = master_recv_2.ypos;
        default:
            break;

        }
    }
    else if(receive_buff[0]==0x55&&receive_buff[1]==0x05)
    {
        recv_data.move_to=receive_buff[2];
    }
}

void CommunicateSend(Master_send_s *send)
{
    static uint8_t master_buffer[20];
    master_buffer[0]=0x6F;
    master_buffer[1]=0x5A;
    master_buffer[2]=send->state;

    USARTSend(master_usart_instance,master_buffer,3,USART_TRANSFER_DMA);
}

typedef enum
{
    WAITING_FOR_FIRST_STOP,
    WAITING_FOR_START,
    WAITING_FOR_NEXT_STOP,
    CAL_DATA_FINISH
}CAL_MASTER_STATE;
    static CAL_MASTER_STATE state=WAITING_FOR_FIRST_STOP;
void CalMasterTimer()
{
    static float next_stop_time,next_start_time;
    static float last_stop_start = 0.0;

    if(state==CAL_DATA_FINISH)
    {
        if(recv_data.is_stop==1)
        {
            // recv_data.is_stop = master_recv_1.is_stop;
            recv_data.remain_time = next_start_time - DWT_GetTimeline_s();
            if(recv_data.remain_time <= 0)
            {
                next_start_time += stop_length+start_length;
                recv_data.is_stop=0;
            }
            recv_data.remain_time = recv_data.remain_time < 0 ? 0 : recv_data.remain_time;

        }
        else
        {
            if(next_stop_time - DWT_GetTimeline_s() <=0)
            {
                next_stop_time +=  stop_length + start_length;
                recv_data.is_stop=1;

                recv_data.color[LAST][0]=recv_data.color[CURRENT][0];
                recv_data.color[LAST][1]=recv_data.color[CURRENT][1];
                recv_data.color[LAST][2]=recv_data.color[CURRENT][2];

                recv_data.color[CURRENT][0]=recv_data.color[FUTURE][0];
                recv_data.color[CURRENT][1]=recv_data.color[FUTURE][1];
                recv_data.color[CURRENT][2]=recv_data.color[FUTURE][2];

                if(recv_data.direction == DIRECTION_CW)
                {
                    recv_data.color[FUTURE][0]=recv_data.color[CURRENT][1];
                    recv_data.color[FUTURE][1]=recv_data.color[CURRENT][2];
                    recv_data.color[FUTURE][2]=recv_data.color[CURRENT][0];
                }
                else if(recv_data.direction == DIRECTION_CCW)
                {
                    recv_data.color[FUTURE][0]=recv_data.color[CURRENT][2];
                    recv_data.color[FUTURE][1]=recv_data.color[CURRENT][0];
                    recv_data.color[FUTURE][2]=recv_data.color[CURRENT][1];
                }
            }
        }

    }
    else if(stop_time_start==0)
    {
        state=WAITING_FOR_FIRST_STOP;
    }
    else if(stop_time_end==0)
    {
        state=WAITING_FOR_START;
    }
    else if(stop_time_start<stop_time_end)
    {
        state=WAITING_FOR_NEXT_STOP;
        last_stop_start = stop_time_start;
    }
    else if(stop_time_start>stop_time_end)
    {
        //此时应当是已经停下
        state=CAL_DATA_FINISH;
        stop_length=4.0f;
        start_length=stop_time_start-stop_time_end;
        use_self_counter=1;
        recv_data.is_stop=1;
        next_stop_time = stop_time_start+stop_length+start_length;
        next_start_time = stop_time_start+stop_length;
    }

}

void EmptyMasterTimer()
{
    state=WAITING_FOR_FIRST_STOP;
    stop_time_start=0;
    stop_time_end=0;
    use_self_counter=0;
}

